package simulator;

import java.util.ArrayList;
import java.util.LinkedList;
import java.util.Queue;

/**
 * This class represents the physical entity of the robot -- and provides direct
 * access to all of the sensors on the board. It is important to note that
 * controls to the robot are *not* implemented here, but in {@link Simulator},
 * for "historical" reasons.
 * 
 * @see Simulator
 */
public class Robot {
	// private fields

	protected ArrayList<Sensor> sensors = new ArrayList<Sensor>();
	protected Servo mountplatform;

	private class Command {
		public static final int STOP = -1;
		public static final int FORWARD = 0;
		public static final int LEFT = 0;
		public static final int RIGHT = 1;
		public static final int BACKWARD = 1;

		public boolean turning;
		public int direction;
		public int arg;

		public Command(boolean turn, int dir, int arg) {
			this.turning = turn;
			this.direction = dir;
			this.arg = arg;
		}

		public Command() {
		}
	}

	private Queue<Command> commands = new LinkedList<Command>();
	private Simulator sim;

	public Robot(Simulator sim) {
		this.sim = sim;
	}

	private Command currentCommand = new Command();

	// acting called by simulator
	// forces changes to config to be regular
	public boolean act(Simulator s) {
		if (commands.peek() == null)
			return false;
		Command c = commands.poll();
		if (c.turning) {
			if (c.direction == Command.LEFT) {
				s.turnLeft();
			} else {
				s.turnRight();
			}
			s.waitAngle(c.arg);
		} else {
			if (c.direction == Command.FORWARD) {
				s.goForward();
			} else if (c.direction == Command.BACKWARD) {
				s.goBackward();
			} else if (c.direction == Command.STOP) {

			}
			s.waitDistance(c.arg);
		}
		System.exit(0);
		return true;
	}

	public void turnLeft(int degrees) {
		turnLeft();
		waitAngle(degrees);
	}

	public void turnLeft() {
		currentCommand.direction = Command.LEFT;
		currentCommand.turning = true;
		sim.turnLeft();
	}

	public void turnRight() {
		currentCommand.direction = Command.RIGHT;
		currentCommand.turning = true;
		sim.turnRight();

	}

	public void turnRight(int degrees) {
		turnRight();
		waitAngle(degrees);
	}

	public void goForward() {
		currentCommand.direction = Command.FORWARD;
		currentCommand.turning = false;
		sim.goForward();
	}

	public void goBackward() {
		currentCommand.direction = Command.BACKWARD;
		currentCommand.turning = false;
		sim.goBackward();
	}

	public void stop() {
		currentCommand.direction = Command.STOP;
		currentCommand.turning = false;
		sim.stop();
	}

	public void setSpeed() {
		// set speed?
	}

	public void waitAngle(int angle) {
		currentCommand.arg = angle;
		commands.add(currentCommand);
		currentCommand = new Command();
		sim.waitAngle(angle);

	}

	public void waitDistance(int dist) {
		currentCommand.arg = dist;
		commands.add(currentCommand);
		currentCommand = new Command();
		sim.waitDistance(dist);
	}

	/**
	 * Provides access to a list configured sensors on the robot
	 * 
	 * @returns an array of {@link Sensor} objects
	 */
	public ArrayList<Sensor> getSensors() {
		return sensors;
	}

	public RangeFinder[] getRangeFinders() {

		ArrayList<RangeFinder> temp = new ArrayList<RangeFinder>();
		for (Sensor s : sensors) {
			if (s.getClass()
					.toString()
					.equals(new RangeFinder(null, 0.0, 0.0, 0.0).getClass()
							.toString()))
				temp.add((RangeFinder) s);
		}
		RangeFinder[] returnval = new RangeFinder[temp.size()];
		for (int i = 0; i < returnval.length; i++) {
			returnval[i] = temp.get(i);
		}
		temp = null;
		return returnval;

	}

	public Servo[] getServos() {

		ArrayList<Servo> temp = new ArrayList<Servo>();
		for (Sensor s : sensors) {
			if (s.getClass().toString()
					.equals(new Servo(null, null, 0).getClass().toString()))
				temp.add((Servo) s);
		}
		Servo[] returnval = new Servo[temp.size()];
		for (int i = 0; i < returnval.length; i++) {
			returnval[i] = temp.get(i);
		}
		temp = null;
		return returnval;

	}

	/**
	 * Adds another sensor to the robot
	 * 
	 * @param s
	 *            the {@link Sensor} to be added
	 */
	public void addSensor(Sensor s) {
		sensors.add(s);
	}

	/**
	 * Reads values from all sensors and returns the updated sensors. This is
	 * called internally from Simulator, and therefore should not be accessed by
	 * actors
	 */
	Sensor[] updateSensors() {
		for (Sensor s : sensors) {
			s.read(68558655);
		}
		Sensor[] s = new Sensor[sensors.size()];

		for (int i = 0; i < sensors.size(); i++) {
			s[i] = sensors.get(i);
		}
		return s;
	}

	public void setServo(Servo t) {
		mountplatform = t;
	}

	public Servo getServo() {
		return mountplatform;
	}

}